/**
 * @file ble_multi_central.c
 * @brief BLE多连接中心设备库实现
 */
#include <errno.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/printk.h>

#include <zephyr/bluetooth/addr.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/sys/byteorder.h>

#include "ble_multi_central.h"

LOG_MODULE_REGISTER(ble_central, 4); // CONFIG_BT_LOG_LEVEL);

static struct ble_central_dev devices[BLE_MAX_CONNECTIONS];
static struct ble_central_config central_config;
static atomic_t discovery_in_progress = ATOMIC_INIT(0);

// 读取回调结构体
struct read_data {
  uint8_t *buffer;
  uint16_t *len;
  uint16_t max_len;
  int err;
  bool complete;
  struct bt_gatt_read_params params;
};

// 服务发现状态跟踪
static struct {
  uint8_t dev_index;
  bool in_progress;
  uint8_t current_svc_index;
  uint8_t current_char_index;
  uint16_t last_char_handle;
  bool discovering_descs;
} discovery_ctx;

// CCC描述符UUID (用于通知和指示)
static struct bt_uuid_16 ccc_uuid = BT_UUID_INIT_16(BT_UUID_GATT_CCC_VAL);

// 互斥锁保护设备数组访问
K_MUTEX_DEFINE(dev_mutex);

static void connected_cb(struct bt_conn *conn, uint8_t err);
static void disconnected_cb(struct bt_conn *conn, uint8_t reason);
static void scan_cb(const bt_addr_le_t *addr, int8_t rssi, uint8_t adv_type,
                    struct net_buf_simple *ad);
static uint8_t discover_services_cb(struct bt_conn *conn,
                                    const struct bt_gatt_attr *attr,
                                    struct bt_gatt_discover_params *params);
static uint8_t discover_chars_cb(struct bt_conn *conn,
                                 const struct bt_gatt_attr *attr,
                                 struct bt_gatt_discover_params *params);
static uint8_t discover_descs_cb(struct bt_conn *conn,
                                 const struct bt_gatt_attr *attr,
                                 struct bt_gatt_discover_params *params);
static uint8_t notify_cb(struct bt_conn *conn,
                         struct bt_gatt_subscribe_params *params,
                         const void *data, uint16_t length);

// 连接回调
static struct bt_conn_cb conn_callbacks = {
    .connected = connected_cb,
    .disconnected = disconnected_cb,
};

// 发现参数
static struct bt_gatt_discover_params discover_params;
static struct bt_gatt_subscribe_params subscribe_params;

// 扫描参数
static struct bt_le_scan_param scan_param = {
    .type = BT_LE_SCAN_TYPE_ACTIVE,
    .options = BT_LE_SCAN_OPT_NONE,
    .interval = BT_GAP_SCAN_FAST_INTERVAL,
    .window = BT_GAP_SCAN_FAST_WINDOW,
};

static bool is_target_uuid(const bt_uuid_t *uuid) {
  if (!uuid) {
    return false;
  }

  for (int i = 0; i < central_config.service_uuid_count; i++) {
    if (!central_config.target_service_uuids[i]) {
      continue;
    }

    if (bt_uuid_cmp(uuid, central_config.target_service_uuids[i]) == 0) {
      return true;
    }
  }

  return false;
}

static uint32_t uuid_to_flag(const bt_uuid_t *uuid) {
  if (!uuid) {
    return 0;
  }

  // 检查是否是标准16位UUID
  if (uuid->type == BT_UUID_TYPE_16) {
    const struct bt_uuid_16 *uuid16 = (const struct bt_uuid_16 *)uuid;
    switch (uuid16->val) {
    case BT_UUID_GATT_PRIMARY_VAL:
      return 0x00000001;
    case BT_UUID_GATT_SECONDARY_VAL:
      return 0x00000002;
    case BT_UUID_GATT_INCLUDE_VAL:
      return 0x00000004;
    case BT_UUID_GATT_CHRC_VAL:
      return 0x00000008;
    case BT_UUID_GATT_CCC_VAL:
      return 0x00000010;
    default:
      break;
    }
  }

  return 0;
}

static bool is_uuid_valid(const bt_uuid_t *uuid) {
  if (!uuid) {
    return false;
  }

  // 验证UUID类型
  switch (uuid->type) {
  case BT_UUID_TYPE_16:
  case BT_UUID_TYPE_32:
  case BT_UUID_TYPE_128:
    return true;
  default:
    LOG_WRN("Unknown UUID type: %u", uuid->type);
    return false;
  }
}

static void log_uuid(const char *prefix, const bt_uuid_t *uuid) {
  if (!uuid) {
    LOG_DBG("%s: NULL UUID", prefix);
    return;
  }

  char uuid_str[BT_UUID_STR_LEN];
  bt_uuid_to_str(uuid, uuid_str, sizeof(uuid_str));
  LOG_DBG("%s: %s (type %d)", prefix, uuid_str, uuid->type);
}

static void copy_uuid(void *dest_union, const bt_uuid_t *src) {
  if (!src) {
    LOG_ERR("Attempted to copy NULL UUID");
    return;
  }

  log_uuid("Copying UUID", src);

  union {
    bt_uuid_t uuid;
    struct bt_uuid_16 uuid16;
    struct bt_uuid_32 uuid32;
    struct bt_uuid_128 uuid128;
  } *dest = dest_union;

  switch (src->type) {
  case BT_UUID_TYPE_16: {
    const struct bt_uuid_16 *src16 = (const struct bt_uuid_16 *)src;
    dest->uuid16.uuid.type = BT_UUID_TYPE_16;
    dest->uuid16.val = src16->val;
    LOG_DBG("Copied 16-bit UUID: 0x%04x", dest->uuid16.val);
  } break;

  case BT_UUID_TYPE_32: {
    const struct bt_uuid_32 *src32 = (const struct bt_uuid_32 *)src;
    dest->uuid32.uuid.type = BT_UUID_TYPE_32;
    dest->uuid32.val = src32->val;
    LOG_DBG("Copied 32-bit UUID: 0x%08x", dest->uuid32.val);
  } break;

  case BT_UUID_TYPE_128: {
    const struct bt_uuid_128 *src128 = (const struct bt_uuid_128 *)src;
    dest->uuid128.uuid.type = BT_UUID_TYPE_128;
    memcpy(dest->uuid128.val, src128->val, 16);
    LOG_DBG("Copied 128-bit UUID");
  } break;

  default:
    LOG_WRN("Unknown UUID type: %u", src->type);
    dest->uuid.type = src->type;
    break;
  }
}

static void clean_device_uuid_data(struct ble_central_dev *dev) {
  if (!dev) {
    return;
  }

  LOG_DBG("Cleaning UUID data for device %d", dev->index);

  // 清理所有服务和UUID数据
  for (int s = 0; s < dev->service_count; s++) {
    struct ble_gatt_service *svc = &dev->services[s];

    // 重置服务UUID
    memset(&svc->uuid_data, 0, sizeof(svc->uuid_data));

    // 重置所有特征值及其UUID
    for (int c = 0; c < svc->char_count; c++) {
      struct ble_gatt_char *chr = &svc->characteristics[c];

      // 重置特征值UUID
      memset(&chr->uuid_data, 0, sizeof(chr->uuid_data));

      // 重置所有描述符及其UUID
      for (int d = 0; d < chr->desc_count; d++) {
        memset(&chr->descriptors[d].uuid_data, 0,
               sizeof(chr->descriptors[d].uuid_data));
        chr->descriptors[d].valid = false;
      }

      chr->desc_count = 0;
      chr->valid = false;
    }

    svc->char_count = 0;
    svc->valid = false;
  }

  dev->service_count = 0;
  dev->discovery_complete = false;
}

// 获取空闲设备槽
static int get_free_device_slot(void) {
  for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
    if (!devices[i].in_use) {
      return i;
    }
  }
  return -1; // 没有空闲槽
}

// 通过连接句柄获取设备索引
static int get_device_index_by_conn(struct bt_conn *conn) {
  for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
    if (devices[i].in_use && devices[i].conn == conn) {
      return i;
    }
  }
  return -1; // 未找到设备
}

// 扫描回调处理
static void scan_cb(const bt_addr_le_t *addr, int8_t rssi, uint8_t adv_type,
                    struct net_buf_simple *ad) {
  // 这里可以添加设备过滤逻辑，例如只处理特定广播数据
  // 目前简单输出日志，实际应用中可根据需要扩展
  char addr_str[BT_ADDR_LE_STR_LEN];
  bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
  LOG_INF("Device found: %s (RSSI %d)", addr_str, rssi);
}

// 服务发现回调
static uint8_t discover_services_cb(struct bt_conn *conn,
                                    const struct bt_gatt_attr *attr,
                                    struct bt_gatt_discover_params *params) {
  if (!attr) {
    LOG_DBG("Service discovery complete");
    // 服务发现完成，开始特征值发现
    if (discovery_ctx.in_progress) {
      // 如果有服务，开始发现第一个服务的特征值
      if (devices[discovery_ctx.dev_index].service_count > 0) {
        discovery_ctx.current_svc_index = 0;
        discovery_ctx.current_char_index = 0;

        struct ble_gatt_service *svc =
            &devices[discovery_ctx.dev_index].services[0];

        discover_params.uuid = NULL;
        discover_params.start_handle = svc->start_handle;
        discover_params.end_handle = svc->end_handle;
        discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
        discover_params.func = discover_chars_cb;

        int err = bt_gatt_discover(conn, &discover_params);
        if (err) {
          LOG_ERR("Characteristic discovery failed (err %d)", err);
          discovery_ctx.in_progress = false;
          devices[discovery_ctx.dev_index].discovery_complete = true;
        }
      } else {
        // 没有目标服务
        discovery_ctx.in_progress = false;
        devices[discovery_ctx.dev_index].discovery_complete = true;
        atomic_clear(&discovery_in_progress); // 重置原子锁
      }
    }
    return BT_GATT_ITER_STOP;
  }

  struct bt_gatt_service_val *service =
      (struct bt_gatt_service_val *)attr->user_data;
  log_uuid("Found service", service->uuid);

  // 检查是否为目标服务
  bool is_target_service = false;
  for (int i = 0; i < central_config.service_uuid_count; i++) {
    if (!central_config.target_service_uuids[i]) {
      continue; // 安全检查
    }

    log_uuid("Comparing with target", central_config.target_service_uuids[i]);

    if (bt_uuid_cmp(service->uuid, central_config.target_service_uuids[i]) ==
        0) {
      is_target_service = true;
      break;
    }
  }

  printk("is_target_service: %d\n", is_target_service);

  if (is_target_service) {
    uint8_t svc_idx = devices[discovery_ctx.dev_index].service_count;
    printk("svc_idx: %d\n", svc_idx);
    if (svc_idx < BLE_MAX_SERVICES_PER_DEV) {
      struct ble_gatt_service *svc =
          &devices[discovery_ctx.dev_index].services[svc_idx];

      copy_uuid(&svc->uuid_data, service->uuid);
      svc->start_handle = attr->handle + 1;
      svc->end_handle = service->end_handle;
      svc->char_count = 0;
      svc->valid = true;

      devices[discovery_ctx.dev_index].service_count++;

      char uuid_str[BT_UUID_STR_LEN];
      bt_uuid_to_str(&svc->uuid_data.uuid, uuid_str, sizeof(uuid_str));

      LOG_DBG("Service found: %s handle %04X, end_handle %04X", uuid_str,
              attr->handle, service->end_handle);
    } else {
      LOG_WRN("Too many services, some will be ignored");
    }
  }

  return BT_GATT_ITER_CONTINUE;
}

// 特征值发现回调
static uint8_t discover_chars_cb(struct bt_conn *conn,
                                 const struct bt_gatt_attr *attr,
                                 struct bt_gatt_discover_params *params) {
  if (!attr) {
    LOG_DBG("Characteristic discovery complete for service %d",
            discovery_ctx.current_svc_index);

    // 移至下一个服务或完成发现
    discovery_ctx.current_svc_index++;
    discovery_ctx.last_char_handle = 0;
    discovery_ctx.discovering_descs = false;

    if (discovery_ctx.current_svc_index <
        devices[discovery_ctx.dev_index].service_count) {
      // 处理下一个服务
      struct ble_gatt_service *svc =
          &devices[discovery_ctx.dev_index]
               .services[discovery_ctx.current_svc_index];

      discover_params.uuid = NULL;
      discover_params.start_handle = svc->start_handle;
      discover_params.end_handle = svc->end_handle;
      discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
      discover_params.func = discover_chars_cb;

      int err = bt_gatt_discover(conn, &discover_params);
      if (err) {
        LOG_ERR("Characteristic discovery failed (err %d)", err);
        discovery_ctx.in_progress = false;
        devices[discovery_ctx.dev_index].discovery_complete = true;
      }
    } else {
      // 所有服务都处理完毕
      LOG_INF("All services and characteristics discovered");
      discovery_ctx.in_progress = false;
      devices[discovery_ctx.dev_index].discovery_complete = true;
      atomic_clear(&discovery_in_progress); // 重置原子锁

      // 启用所有可通知特征的通知
      for (int s = 0; s < devices[discovery_ctx.dev_index].service_count; s++) {
        struct ble_gatt_service *svc =
            &devices[discovery_ctx.dev_index].services[s];
        for (int c = 0; c < svc->char_count; c++) {
          struct ble_gatt_char *chr = &svc->characteristics[c];
          if (chr->properties & BT_GATT_CHRC_NOTIFY) {
            ble_central_enable_notifications(discovery_ctx.dev_index,
                                             &svc->uuid_data.uuid,
                                             &chr->uuid_data.uuid, true);
          }
        }
      }
    }

    return BT_GATT_ITER_STOP;
  }

  // 确保不会重复处理相同特征值
  if (attr->handle <= discovery_ctx.last_char_handle) {
    // 跳过已处理过的特征值
    return BT_GATT_ITER_CONTINUE;
  }

  struct bt_gatt_chrc *chrc = (struct bt_gatt_chrc *)attr->user_data;

  struct ble_gatt_service *svc =
      &devices[discovery_ctx.dev_index]
           .services[discovery_ctx.current_svc_index];
  uint8_t char_idx = svc->char_count;

  LOG_DBG("char_idx: %d", char_idx);

  if (char_idx < BLE_MAX_CHARS_PER_SERVICE) {
    struct ble_gatt_char *chr = &svc->characteristics[char_idx];

    copy_uuid(&chr->uuid_data, chrc->uuid);
    chr->handle = attr->handle;
    chr->value_handle = chrc->value_handle;
    chr->properties = chrc->properties;
    chr->ccc_handle = 0;
    chr->notify_enabled = false;
    chr->indicate_enabled = false;
    chr->desc_count = 0;
    chr->valid = true;

    svc->char_count++;

    LOG_DBG("Characteristic found: handle %04X, val_handle %04X, props 0x%02x",
            attr->handle, chrc->value_handle, chrc->properties);

    // 更新最后处理的特征值句柄
    discovery_ctx.last_char_handle = attr->handle;
    discovery_ctx.current_char_index = char_idx;

    // 发现描述符
    if (char_idx < BLE_MAX_CHARS_PER_SERVICE) {
      // 计算描述符搜索范围
      uint16_t start = chrc->value_handle + 1;
      uint16_t end;

      // 计算结束位置：
      // 如果是服务中的最后一个特征值，则使用服务结束句柄，
      // 否则，找出下一个特征值句柄（得继续寻找下一个特征值，而不是用现有的）
      //
      // 为了安全起见，先设置为服务结束句柄
      end = svc->end_handle;

      // 查找当前服务中是否还有其它特征值
      bool more_chars = false;
      struct bt_gatt_discover_params temp_params;
      memcpy(&temp_params, params, sizeof(temp_params));

      // 修改起始位置从当前特征值之后开始查找
      temp_params.start_handle = attr->handle + 1;

      // 如果起始位置有效且在服务范围内，可能还有更多特征值
      if (temp_params.start_handle <= svc->end_handle &&
          temp_params.start_handle > attr->handle) {
        more_chars = false;
      }

      LOG_DBG("Discovering descriptors for characteristic %d, range %04X-%04X",
              char_idx, start, end);

      if (start <= end) {
        discovery_ctx.discovering_descs = true;

        discover_params.uuid = NULL;
        discover_params.start_handle = start;
        discover_params.end_handle = end;
        discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
        discover_params.func = discover_descs_cb;

        // discovery_ctx.current_char_index = char_idx;

        int err = bt_gatt_discover(conn, &discover_params);
        if (err) {
          LOG_ERR("Descriptor discovery failed (err %d)", err);
          discovery_ctx.discovering_descs = false;
          return BT_GATT_ITER_STOP;
        }

        return BT_GATT_ITER_STOP; // 等待描述符发现完成
      }
    }
  } else {
    LOG_WRN("Too many characteristics in service, some will be ignored");
  }

  return BT_GATT_ITER_CONTINUE;
}

// 描述符发现回调
static uint8_t discover_descs_cb(struct bt_conn *conn,
                                 const struct bt_gatt_attr *attr,
                                 struct bt_gatt_discover_params *params) {
  if (!attr) {
    LOG_DBG("Descriptor discovery complete");

    discovery_ctx.discovering_descs = false;

    // 为当前特征值启用通知
    struct ble_gatt_service *svc =
        &devices[discovery_ctx.dev_index]
             .services[discovery_ctx.current_svc_index];
    struct ble_gatt_char *chr =
        &svc->characteristics[discovery_ctx.current_char_index];

    if ((chr->properties & BT_GATT_CHRC_NOTIFY) && chr->ccc_handle != 0) {
      // 自动使能通知
      static struct bt_gatt_subscribe_params sub_params = {0};

      sub_params.value_handle = chr->value_handle;
      sub_params.ccc_handle = chr->ccc_handle;
      sub_params.value = BT_GATT_CCC_NOTIFY;
      sub_params.notify = notify_cb;
      sub_params.subscribe = NULL;

      int err = bt_gatt_subscribe(conn, &sub_params);
      if (err && err != -EALREADY) {
        LOG_ERR("Auto-subscribe to notifications failed (err %d)", err);
      } else {
        LOG_INF("Auto-enabled notifications for characteristic (handle: %04X)",
                chr->value_handle);
        chr->notify_enabled = true;
      }
    }

    // 继续发现特在值，但从上次发现的特征值之后开始
    discover_params.uuid = NULL;
    discover_params.start_handle =
        discovery_ctx.last_char_handle; // 从上次位置继续
    discover_params.end_handle = svc->end_handle;
    discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
    discover_params.func = discover_chars_cb;
    LOG_DBG("Returning to characteristic discovery, start_handle %04X, "
            "end_handle %04x",
            discover_params.start_handle, discover_params.end_handle);

    int err = bt_gatt_discover(conn, &discover_params);
    if (err) {
      LOG_ERR("Returning to characteristic discovery failed (err %d)", err);
      discovery_ctx.in_progress = false;
      devices[discovery_ctx.dev_index].discovery_complete = true;
    }

    return BT_GATT_ITER_STOP;
  }

  struct ble_gatt_service *svc =
      &devices[discovery_ctx.dev_index]
           .services[discovery_ctx.current_svc_index];
  struct ble_gatt_char *chr =
      &svc->characteristics[discovery_ctx.current_char_index];

  uint8_t desc_idx = chr->desc_count;
  if (desc_idx < BLE_MAX_DESCS_PER_CHAR) {
    struct ble_gatt_desc *desc = &chr->descriptors[desc_idx];

    copy_uuid(&desc->uuid_data, attr->uuid);
    desc->handle = attr->handle;
    desc->valid = true;

    chr->desc_count++;

    LOG_DBG("Descriptor found: handle %04X", attr->handle);

    // 如果找到CCC描述符，记录下来
    if (bt_uuid_cmp(attr->uuid, &ccc_uuid.uuid) == 0) {
      LOG_DBG("CCC descriptor found for characteristic");
      chr->ccc_handle = attr->handle; // 保存CCC句柄
    }
  } else {
    LOG_WRN("Too many descriptors, some will be ignored");
  }

  return BT_GATT_ITER_CONTINUE;
}

// 通知回调
static uint8_t notify_cb(struct bt_conn *conn,
                         struct bt_gatt_subscribe_params *params,
                         const void *data, uint16_t length) {
  if (!data) {
    LOG_DBG("Notification/Indication unsubscribed");
    return BT_GATT_ITER_CONTINUE;
  }

  int dev_idx = get_device_index_by_conn(conn);
  if (dev_idx < 0) {
    LOG_ERR("Received notification from unknown device");
    return BT_GATT_ITER_CONTINUE;
  }

  // 查找对应的特征值
  for (int s = 0; s < devices[dev_idx].service_count; s++) {
    struct ble_gatt_service *svc = &devices[dev_idx].services[s];
    for (int c = 0; c < svc->char_count; c++) {
      struct ble_gatt_char *chr = &svc->characteristics[c];
      if (chr->value_handle == params->value_handle) {
        // 找到匹配特征值
        if (central_config.data_cb) {
          central_config.data_cb(&devices[dev_idx], chr, data, length,
                                 central_config.user_data);
        }
        return BT_GATT_ITER_CONTINUE;
      }
    }
  }

  LOG_WRN("Received notification for unknown characteristic (handle %u)",
          params->value_handle);
  return BT_GATT_ITER_CONTINUE;
}

// ------------- 公开API实现 -------------

int ble_central_start_scan(void) {
  int err = bt_le_scan_start(&scan_param, scan_cb);
  if (err) {
    LOG_ERR("Scanning failed to start (err %d)", err);
    return err;
  }

  LOG_INF("Scanning started");
  return 0;
}

int ble_central_stop_scan(void) {
  int err = bt_le_scan_stop();
  if (err) {
    LOG_ERR("Scanning failed to stop (err %d)", err);
    return err;
  }

  LOG_INF("Scanning stopped");
  return 0;
}

struct ble_central_dev *ble_central_get_device(uint8_t dev_index) {
  if (dev_index >= BLE_MAX_CONNECTIONS) {
    return NULL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  struct ble_central_dev *dev = NULL;
  if (devices[dev_index].in_use) {
    dev = &devices[dev_index];
  }

  k_mutex_unlock(&dev_mutex);

  return dev;
}

uint8_t ble_central_get_connected_count(void) {
  uint8_t count = 0;

  k_mutex_lock(&dev_mutex, K_FOREVER);

  for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
    if (devices[i].in_use && devices[i].connected) {
      count++;
    }
  }

  k_mutex_unlock(&dev_mutex);

  return count;
}

int ble_central_discover_services(uint8_t dev_index) {
  if (dev_index >= BLE_MAX_CONNECTIONS) {
    return -EINVAL;
  }

  // 防止重入或并发调用
  if (!atomic_cas(&discovery_in_progress, 0, 1)) {
    LOG_WRN("Service discovery already in progress");
    return -EBUSY;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use || !devices[dev_index].connected) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  // if (discovery_ctx.in_progress) {
  //   k_mutex_unlock(&dev_mutex);
  //   return -EBUSY;
  // }

  // 准备发现上下文
  discovery_ctx.dev_index = dev_index;
  discovery_ctx.in_progress = true;
  discovery_ctx.current_svc_index = 0;
  discovery_ctx.last_char_handle = 0;
  discovery_ctx.discovering_descs = false;

  // 重置服务计数
  devices[dev_index].service_count = 0;
  devices[dev_index].discovery_complete = false;

  struct bt_conn *conn = devices[dev_index].conn;

  k_mutex_unlock(&dev_mutex);

  // 开始服务发现
  discover_params.uuid = NULL;
  discover_params.func = discover_services_cb;
  discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
  discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
  discover_params.type = BT_GATT_DISCOVER_PRIMARY;

  int err = bt_gatt_discover(conn, &discover_params);
  if (err) {
    LOG_ERR("Service discovery failed (err %d)", err);
    discovery_ctx.in_progress = false;
    atomic_clear(&discovery_in_progress); // 重置原子锁
    return err;
  }

  return 0;
}

struct ble_gatt_service *ble_central_find_service(uint8_t dev_index,
                                                  const bt_uuid_t *uuid) {
  if (dev_index >= BLE_MAX_CONNECTIONS || !is_uuid_valid(uuid)) {
    return NULL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use) {
    k_mutex_unlock(&dev_mutex);
    return NULL;
  }

  struct ble_gatt_service *result = NULL;

  for (int i = 0; i < devices[dev_index].service_count; i++) {
    struct ble_gatt_service *svc = &devices[dev_index].services[i];
    // 使用 bt_uuid_cmp 比较 UUID 数据联合体中的 uuid 字段
    if (svc->valid && bt_uuid_cmp(&svc->uuid_data.uuid, uuid) == 0) {
      result = svc;
      break;
    }
  }

  k_mutex_unlock(&dev_mutex);

  return result;
}

struct ble_gatt_char *
ble_central_find_characteristic(struct ble_gatt_service *service,
                                const bt_uuid_t *uuid) {
  if (!service || !is_uuid_valid(uuid)) {
    return NULL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  struct ble_gatt_char *result = NULL;

  for (int i = 0; i < service->char_count; i++) {
    struct ble_gatt_char *chr = &service->characteristics[i];
    if (chr->valid && bt_uuid_cmp(&chr->uuid_data.uuid, uuid) == 0) {
      result = chr;
      break;
    }
  }

  k_mutex_unlock(&dev_mutex);

  return result;
}

struct ble_gatt_desc *
ble_central_find_descriptor(struct ble_gatt_char *characteristic,
                            const bt_uuid_t *uuid) {
  if (!characteristic || !is_uuid_valid(uuid)) {
    return NULL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  struct ble_gatt_desc *result = NULL;

  for (int i = 0; i < characteristic->desc_count; i++) {
    struct ble_gatt_desc *desc = &characteristic->descriptors[i];
    if (desc->valid && bt_uuid_cmp(&desc->uuid_data.uuid, uuid) == 0) {
      result = desc;
      break;
    }
  }

  k_mutex_unlock(&dev_mutex);

  return result;
}

int ble_central_write_characteristic(uint8_t dev_index,
                                     const bt_uuid_t *service_uuid,
                                     const bt_uuid_t *char_uuid,
                                     const uint8_t *data, uint16_t len,
                                     bool response) {
  if (dev_index >= BLE_MAX_CONNECTIONS || !service_uuid ||
      !is_uuid_valid(char_uuid) || !data || len == 0) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use || !devices[dev_index].connected) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct bt_conn *conn = devices[dev_index].conn;

  // 查找服务和特征值
  struct ble_gatt_service *svc = NULL;
  for (int i = 0; i < devices[dev_index].service_count; i++) {
    if (bt_uuid_cmp(&devices[dev_index].services[i].uuid_data.uuid,
                    service_uuid) == 0) {
      svc = &devices[dev_index].services[i];
      break;
    }
  }

  if (!svc) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct ble_gatt_char *chr = NULL;
  for (int i = 0; i < svc->char_count; i++) {
    if (bt_uuid_cmp(&svc->characteristics[i].uuid_data.uuid, char_uuid) == 0) {
      chr = &svc->characteristics[i];
      break;
    }
  }

  if (!chr) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  uint16_t handle = chr->value_handle;

  printk("Write handle: %04X\n", handle);

  k_mutex_unlock(&dev_mutex);

  // 写入特征值
  int err;
  struct bt_gatt_write_params write_params = {0};
  write_params.func = NULL;
  write_params.data = data;
  write_params.length = len;
  write_params.offset = 0;
  write_params.handle = handle;

  if (response) {
    err = bt_gatt_write(conn, &write_params);
  } else {
    err = bt_gatt_write_without_response(conn, handle, data, len, false);
  }

  if (err) {
    LOG_ERR("Write failed (err %d)", err);
  }

  return err;
}

// 读取回调
static uint8_t read_cb(struct bt_conn *conn, uint8_t err,
                       struct bt_gatt_read_params *params, const void *data,
                       uint16_t length) {
  struct read_data *read_data = CONTAINER_OF(params, struct read_data, params);

  if (err) {
    read_data->err = err;
  } else if (data) {
    if (length <= read_data->max_len) {
      memcpy(read_data->buffer, data, length);
      *read_data->len = length;
    } else {
      read_data->err = -ENOMEM;
    }
  }

  read_data->complete = true;

  return BT_GATT_ITER_STOP;
}

int ble_central_read_characteristic(uint8_t dev_index,
                                    const bt_uuid_t *service_uuid,
                                    const bt_uuid_t *char_uuid, uint8_t *data,
                                    uint16_t *len) {
  if (dev_index >= BLE_MAX_CONNECTIONS || !is_uuid_valid(service_uuid) ||
      !is_uuid_valid(char_uuid) || !data || !len || *len == 0) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use || !devices[dev_index].connected) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct bt_conn *conn = devices[dev_index].conn;

  // 查找服务和特征值
  struct ble_gatt_service *svc = NULL;
  for (int i = 0; i < devices[dev_index].service_count; i++) {
    if (bt_uuid_cmp(&devices[dev_index].services[i].uuid_data.uuid,
                    service_uuid) == 0) {
      svc = &devices[dev_index].services[i];
      break;
    }
  }

  if (!svc) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct ble_gatt_char *chr = NULL;
  for (int i = 0; i < svc->char_count; i++) {
    if (bt_uuid_cmp(&svc->characteristics[i].uuid_data.uuid, char_uuid) == 0) {
      chr = &svc->characteristics[i];
      break;
    }
  }

  if (!chr) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  uint16_t handle = chr->value_handle;

  k_mutex_unlock(&dev_mutex);

  // 设置读取参数
  struct read_data read_data;
  read_data.buffer = data;
  read_data.len = len;
  read_data.max_len = *len;
  read_data.err = 0;
  read_data.complete = false;
  read_data.params.func = read_cb;
  read_data.params.handle_count = 1;
  read_data.params.single.handle = handle;
  read_data.params.single.offset = 0;

  int err = bt_gatt_read(conn, &read_data.params);
  if (err) {
    LOG_ERR("Read failed (err %d)", err);
    return err;
  }

  // 等待读取完成
  while (!read_data.complete) {
    k_yield();
  }

  return read_data.err;
}

int ble_central_enable_notifications(uint8_t dev_index,
                                     const bt_uuid_t *service_uuid,
                                     const bt_uuid_t *char_uuid, bool enable) {
  if (dev_index >= BLE_MAX_CONNECTIONS || !is_uuid_valid(service_uuid) ||
      !is_uuid_valid(char_uuid)) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use || !devices[dev_index].connected) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct bt_conn *conn = devices[dev_index].conn;

  // 查找服务和特征值
  struct ble_gatt_service *svc = NULL;
  for (int i = 0; i < devices[dev_index].service_count; i++) {
    if (bt_uuid_cmp(&devices[dev_index].services[i].uuid_data.uuid,
                    service_uuid) == 0) {
      svc = &devices[dev_index].services[i];
      break;
    }
  }

  if (!svc) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct ble_gatt_char *chr = NULL;
  for (int i = 0; i < svc->char_count; i++) {
    if (bt_uuid_cmp(&svc->characteristics[i].uuid_data.uuid, char_uuid) == 0) {
      chr = &svc->characteristics[i];
      break;
    }
  }

  if (!chr) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  // 检查特征值是否支持通知
  if (!(chr->properties & BT_GATT_CHRC_NOTIFY)) {
    LOG_ERR("Characteristic does not support notifications");
    k_mutex_unlock(&dev_mutex);
    return -ENOTSUP;
  }

  // 查找CCC描述符
  // uint16_t ccc_handle = 0;
  // for (int i = 0; i < chr->desc_count; i++)
  // {
  //     if (bt_uuid_cmp(&chr->descriptors[i].uuid, &ccc_uuid.uuid) == 0)
  //     {
  //         ccc_handle = chr->descriptors[i].handle;
  //         break;
  //     }
  // }

  // printk("ccc_handle: %04X\n", ccc_handle);

  // if (ccc_handle == 0)
  // {
  //     LOG_ERR("CCC descriptor not found");
  //     k_mutex_unlock(&dev_mutex);
  //     return -ENOTSUP;
  // }
  if (chr->ccc_handle == 0) {
    LOG_ERR("CCC descriptor not found");
    k_mutex_unlock(&dev_mutex);
    return -ENOTSUP;
  }

  k_mutex_unlock(&dev_mutex);

  // 设置通知
  static struct bt_gatt_subscribe_params sub_params;

  if (enable) {
    if (chr->notify_enabled) {
      return 0; // 已经启用
    }

    sub_params.value_handle = chr->value_handle;
    sub_params.ccc_handle = chr->ccc_handle;
    sub_params.value = BT_GATT_CCC_NOTIFY;
    sub_params.notify = notify_cb;
    sub_params.subscribe = NULL;

    int err = bt_gatt_subscribe(conn, &sub_params);
    if (err && err != -EALREADY) {
      LOG_ERR("Subscribe failed (err %d)", err);
      return err;
    }

    chr->notify_enabled = true;
    LOG_DBG("Subscribe success, ccc_handle %04X", chr->ccc_handle);
  } else {
    if (!chr->notify_enabled) {
      return 0; // 已经禁用
    }

    int err = bt_gatt_unsubscribe(conn, &sub_params);
    if (err) {
      LOG_ERR("Unsubscribe failed (err %d)", err);
      return err;
    }

    chr->notify_enabled = false;
  }

  return 0;
}

int ble_central_enable_indications(uint8_t dev_index,
                                   const bt_uuid_t *service_uuid,
                                   const bt_uuid_t *char_uuid, bool enable) {
  if (dev_index >= BLE_MAX_CONNECTIONS || !is_uuid_valid(service_uuid) ||
      !is_uuid_valid(char_uuid)) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use || !devices[dev_index].connected) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct bt_conn *conn = devices[dev_index].conn;

  // 查找服务和特征值
  struct ble_gatt_service *svc = NULL;
  for (int i = 0; i < devices[dev_index].service_count; i++) {
    if (bt_uuid_cmp(&devices[dev_index].services[i].uuid_data.uuid,
                    service_uuid) == 0) {
      svc = &devices[dev_index].services[i];
      break;
    }
  }

  if (!svc) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  struct ble_gatt_char *chr = NULL;
  for (int i = 0; i < svc->char_count; i++) {
    if (bt_uuid_cmp(&svc->characteristics[i].uuid_data.uuid, char_uuid) == 0) {
      chr = &svc->characteristics[i];
      break;
    }
  }

  if (!chr) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  // 检查特征值是否支持指示
  if (!(chr->properties & BT_GATT_CHRC_INDICATE)) {
    LOG_ERR("Characteristic does not support indications");
    k_mutex_unlock(&dev_mutex);
    return -ENOTSUP;
  }

  // 查找CCC描述符
  // uint16_t ccc_handle = 0;
  // for (int i = 0; i < chr->desc_count; i++)
  // {
  //     if (bt_uuid_cmp(&chr->descriptors[i].uuid, &ccc_uuid.uuid) == 0)
  //     {
  //         ccc_handle = chr->descriptors[i].handle;
  //         break;
  //     }
  // }

  // if (ccc_handle == 0)
  // {
  //     LOG_ERR("CCC descriptor not found");
  //     k_mutex_unlock(&dev_mutex);
  //     return -ENOTSUP;
  // }
  if (chr->ccc_handle == 0) {
    LOG_ERR("CCC descriptor handle not found");
    k_mutex_unlock(&dev_mutex);
    return -ENOTSUP;
  }

  k_mutex_unlock(&dev_mutex);

  // 设置指示
  static struct bt_gatt_subscribe_params sub_params;

  if (enable) {
    if (chr->indicate_enabled) {
      return 0; // 已经启用
    }

    sub_params.value_handle = chr->value_handle;
    sub_params.ccc_handle = chr->ccc_handle;
    sub_params.value = BT_GATT_CCC_INDICATE;
    sub_params.notify = notify_cb;
    sub_params.subscribe = NULL;

    int err = bt_gatt_subscribe(conn, &sub_params);
    if (err && err != -EALREADY) {
      LOG_ERR("Subscribe failed (err %d)", err);
      return err;
    }

    chr->indicate_enabled = true;
  } else {
    if (!chr->indicate_enabled) {
      return 0; // 已经禁用
    }

    int err = bt_gatt_unsubscribe(conn, &sub_params);
    if (err) {
      LOG_ERR("Unsubscribe failed (err %d)", err);
      return err;
    }

    chr->indicate_enabled = false;
  }

  return 0;
}

// 连接超时处理函数
static void connect_timeout_handler(struct k_work *work) {
  struct k_work_delayable *dwork = k_work_delayable_from_work(work);
  struct ble_central_dev *dev =
      CONTAINER_OF(dwork, struct ble_central_dev, connect_timeout_work);

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (dev->connecting && !dev->connected) {
    LOG_WRN("Connection timeout for device %d", dev->index);

    // 如果连接仍在进行中但已超时，则断开
    if (dev->conn) {
      bt_conn_disconnect(dev->conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
      bt_conn_unref(dev->conn);
      dev->conn = NULL;
    }

    dev->connecting = false;

    // 如果启用了自动重连且未超过最大尝试次数
    if (dev->auto_reconnect &&
        dev->reconnect_attempts < BLE_MAX_AUTO_CONNECT_RETRIES) {
      LOG_INF("Scheduling reconnection attempt %d/%d for device %d",
              dev->reconnect_attempts + 1, BLE_MAX_AUTO_CONNECT_RETRIES,
              dev->index);

      k_work_schedule(&dev->retry_work, BLE_AUTO_CONNECT_RETRY_INTERVAL);
    } else if (dev->reconnect_attempts >= BLE_MAX_AUTO_CONNECT_RETRIES) {
      LOG_WRN("Max reconnection attempts reached for device %d", dev->index);
      // 重置重连尝试次数，等待用户手动重连
      dev->reconnect_attempts = 0;
    }
  }

  k_mutex_unlock(&dev_mutex);
}

// 重试连接处理函数
static void retry_connect_handler(struct k_work *work) {
  struct k_work_delayable *dwork = k_work_delayable_from_work(work);
  struct ble_central_dev *dev =
      CONTAINER_OF(dwork, struct ble_central_dev, retry_work);

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (dev->in_use && !dev->connected && !dev->connecting &&
      dev->auto_reconnect) {
    LOG_INF("Attempting to reconnect to device %d (attempt %d/%d)", dev->index,
            dev->reconnect_attempts + 1, BLE_MAX_AUTO_CONNECT_RETRIES);

    // 增加重连计数
    dev->reconnect_attempts++;
    dev->connecting = true;

    // 保存设备地址用于后续使用
    bt_addr_le_t addr;
    bt_addr_le_copy(&addr, &dev->addr);

    k_mutex_unlock(&dev_mutex);

    // 停止扫描以提高连接成功率
    bt_le_scan_stop();

    // 尝试重新连接
    struct bt_conn *conn;
    int err = bt_conn_le_create(&addr, BT_CONN_LE_CREATE_CONN,
                                BT_LE_CONN_PARAM_DEFAULT, &conn);

    if (err) {
      LOG_ERR("Reconnection attempt failed (err %d)", err);

      k_mutex_lock(&dev_mutex, K_FOREVER);
      dev->connecting = false;

      // 如果还有重试次数，安排下次重试
      if (dev->reconnect_attempts < BLE_MAX_AUTO_CONNECT_RETRIES) {
        k_work_schedule(&dev->retry_work, BLE_AUTO_CONNECT_RETRY_INTERVAL);
      } else {
        LOG_WRN("Max reconnection attempts reached for device %d", dev->index);
        dev->reconnect_attempts = 0;
      }

      k_mutex_unlock(&dev_mutex);
    } else {
      // 连接创建成功，但actual connection establishment由connected_cb处理
      bt_conn_unref(conn);

      // 启动连接超时定时器
      k_mutex_lock(&dev_mutex, K_FOREVER);
      k_work_schedule(&dev->connect_timeout_work, BLE_CONNECTION_TIMEOUT);
      k_mutex_unlock(&dev_mutex);
    }

    return;
  }

  k_mutex_unlock(&dev_mutex);
}

// 修改初始化函数，添加工作队列初始化
int ble_central_init(const struct ble_central_config *config) {
  if (!config) {
    return -EINVAL;
  }

  // 检查传入的UUID数组是否有效
  if (config->service_uuid_count > 0 && !config->target_service_uuids) {
    return -EINVAL;
  }

  // 保存配置
  memcpy(&central_config, config, sizeof(struct ble_central_config));

  // 验证传入的UUID指针是否有效
  for (int i = 0; i < central_config.service_uuid_count; i++) {
    if (!central_config.target_service_uuids[i]) {
      LOG_ERR("Invalid UUID pointer at index %d", i);
      return -EINVAL;
    }

    // 额外的验证，确保UUID类型有效
    if (central_config.target_service_uuids[i]->type > BT_UUID_TYPE_128) {
      LOG_ERR("Invalid UUID type at index %d: %d", i,
              central_config.target_service_uuids[i]->type);
      return -EINVAL;
    }
  }

  // 初始化设备数组
  memset(devices, 0, sizeof(devices));

  // 初始化每个设备的工作队列和状态
  for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
    k_work_init_delayable(&devices[i].connect_timeout_work,
                          connect_timeout_handler);
    k_work_init_delayable(&devices[i].retry_work, retry_connect_handler);
    devices[i].auto_reconnect = false;
    devices[i].reconnect_attempts = 0;
    devices[i].connecting = false;
  }

  // 初始化发现上下文
  memset(&discovery_ctx, 0, sizeof(discovery_ctx));

  // 注册连接回调
  bt_conn_cb_register(&conn_callbacks);

  LOG_INF("BLE Multi Central initialized");

  return 0;
}

// 修改连接回调，处理连接状态变化
static void connected_cb(struct bt_conn *conn, uint8_t err) {
  char addr_str[BT_ADDR_LE_STR_LEN];
  bt_addr_le_to_str(bt_conn_get_dst(conn), addr_str, sizeof(addr_str));

  if (err) {
    LOG_ERR("Connection to %s failed (err %u)", addr_str, err);

    // 查找处于连接中的设备
    k_mutex_lock(&dev_mutex, K_FOREVER);

    for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
      if (devices[i].in_use && devices[i].connecting &&
          bt_addr_le_cmp(&devices[i].addr, bt_conn_get_dst(conn)) == 0) {

        LOG_INF("Connection failed for device %d", i);

        // 取消超时计时器
        k_work_cancel_delayable(&devices[i].connect_timeout_work);

        devices[i].connecting = false;

        // 如果有连接引用，释放它
        if (devices[i].conn) {
          bt_conn_unref(devices[i].conn);
          devices[i].conn = NULL;
        }

        // 如果启用了自动重连且未超过最大尝试次数
        if (devices[i].auto_reconnect &&
            devices[i].reconnect_attempts < BLE_MAX_AUTO_CONNECT_RETRIES) {

          LOG_INF("Scheduling reconnection attempt %d/%d for device %d",
                  devices[i].reconnect_attempts + 1,
                  BLE_MAX_AUTO_CONNECT_RETRIES, i);

          k_work_schedule(&devices[i].retry_work,
                          BLE_AUTO_CONNECT_RETRY_INTERVAL);
        } else if (devices[i].reconnect_attempts >=
                   BLE_MAX_AUTO_CONNECT_RETRIES) {
          LOG_WRN("Max reconnection attempts reached for device %d", i);
          devices[i].reconnect_attempts = 0;
        }

        break;
      }
    }

    k_mutex_unlock(&dev_mutex);
    return;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  int idx = get_device_index_by_conn(conn);
  if (idx < 0) {
    // 查找处于连接中的设备
    for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
      if (devices[i].in_use && devices[i].connecting &&
          bt_addr_le_cmp(&devices[i].addr, bt_conn_get_dst(conn)) == 0) {
        idx = i;
        break;
      }
    }

    // 如果仍未找到匹配的设备，则查找空闲槽
    if (idx < 0) {
      idx = get_free_device_slot();
      if (idx < 0) {
        LOG_ERR("No free device slots");
        bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
        k_mutex_unlock(&dev_mutex);
        return;
      }

      devices[idx].in_use = true;
      bt_addr_le_copy(&devices[idx].addr, bt_conn_get_dst(conn));
    }

    devices[idx].conn = bt_conn_ref(conn);
    devices[idx].index = idx;
  }

  // 取消超时计时器
  k_work_cancel_delayable(&devices[idx].connect_timeout_work);

  devices[idx].connected = true;
  devices[idx].connecting = false;
  devices[idx].reconnect_attempts = 0; // 连接成功，重置重连计数
  devices[idx].discovery_complete = false;

  LOG_INF("Connected to device %d: %s", idx, addr_str);

  // 调用用户回调
  if (central_config.connect_cb) {
    central_config.connect_cb(&devices[idx], central_config.user_data);
  }

  k_mutex_unlock(&dev_mutex);
}

// 修改断开连接回调，处理自动重连
static void disconnected_cb(struct bt_conn *conn, uint8_t reason) {
  char addr_str[BT_ADDR_LE_STR_LEN];
  bt_addr_le_to_str(bt_conn_get_dst(conn), addr_str, sizeof(addr_str));

  k_mutex_lock(&dev_mutex, K_FOREVER);

  int idx = get_device_index_by_conn(conn);
  if (idx >= 0) {
    LOG_INF("Disconnected from device %d: %s (reason %u)", idx, addr_str,
            reason);

    bool was_connected = devices[idx].connected;
    devices[idx].connected = false;

    // 调用用户回调
    if (central_config.disconnect_cb) {
      central_config.disconnect_cb(&devices[idx], reason,
                                   central_config.user_data);
    }

    // 释放连接引用
    bt_conn_unref(devices[idx].conn);
    devices[idx].conn = NULL;

    // 清理UUID数据
    clean_device_uuid_data(&devices[idx]);

    // 如果断开连接不是由用户主动发起(HCI_ERR_REMOTE_USER_TERM_CONN)，
    // 并且启用了自动重连，则尝试重新连接
    if (was_connected && reason != BT_HCI_ERR_REMOTE_USER_TERM_CONN &&
        devices[idx].auto_reconnect && !devices[idx].connecting) {

      LOG_INF("Scheduling reconnection for device %d", idx);
      devices[idx].reconnect_attempts = 0; // 重置重连计数
      k_work_schedule(&devices[idx].retry_work,
                      BLE_AUTO_CONNECT_RETRY_INTERVAL);
    } else {
      // 仅当不进行自动重连时才释放设备槽
      if (!devices[idx].auto_reconnect) {
        devices[idx].in_use = false;
      }
    }
  }

  k_mutex_unlock(&dev_mutex);
}

// 修改connect函数，添加超时处理
int ble_central_connect(const bt_addr_le_t *addr, uint8_t *dev_index) {
  if (!addr) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  // 检查是否已经连接
  for (int i = 0; i < BLE_MAX_CONNECTIONS; i++) {
    if (devices[i].in_use && bt_addr_le_cmp(&devices[i].addr, addr) == 0) {
      if (devices[i].connected) {
        LOG_INF("Already connected to device");
        if (dev_index) {
          *dev_index = i;
        }
        k_mutex_unlock(&dev_mutex);
        return 0;
      } else if (devices[i].connecting) {
        LOG_INF("Connection already in progress");
        if (dev_index) {
          *dev_index = i;
        }
        k_mutex_unlock(&dev_mutex);
        return BLE_ERR_BUSY;
      }
    }
  }

  // 获取空闲槽
  int idx = get_free_device_slot();
  if (idx < 0) {
    LOG_ERR("No free device slots");
    k_mutex_unlock(&dev_mutex);
    return -ENOMEM;
  }

  // 初始化设备槽
  memset(&devices[idx], 0, sizeof(struct ble_central_dev));
  bt_addr_le_copy(&devices[idx].addr, addr);
  devices[idx].in_use = true;
  devices[idx].index = idx;
  devices[idx].connecting = true;
  devices[idx].reconnect_attempts = 0;

  // 保存索引以供调用者使用
  if (dev_index) {
    *dev_index = idx;
  }

  // 初始化工作队列（仅在ble_central_init中未完成时需要）
  if (!k_work_delayable_is_pending(&devices[idx].connect_timeout_work)) {
    k_work_init_delayable(&devices[idx].connect_timeout_work,
                          connect_timeout_handler);
  }
  if (!k_work_delayable_is_pending(&devices[idx].retry_work)) {
    k_work_init_delayable(&devices[idx].retry_work, retry_connect_handler);
  }

  // 设置连接超时
  k_work_schedule(&devices[idx].connect_timeout_work, BLE_CONNECTION_TIMEOUT);

  k_mutex_unlock(&dev_mutex);

  // 停止扫描，以提高连接成功率
  bt_le_scan_stop();

  // 创建连接
  struct bt_conn *conn;
  int err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
                              BT_LE_CONN_PARAM_DEFAULT, &conn);
  if (err) {
    LOG_ERR("Connection creation failed (err %d)", err);

    k_mutex_lock(&dev_mutex, K_FOREVER);
    devices[idx].connecting = false;
    k_work_cancel_delayable(&devices[idx].connect_timeout_work);
    devices[idx].in_use = false;
    k_mutex_unlock(&dev_mutex);

    return err;
  }

  // 连接创建成功，但连接建立由connected_cb处理
  bt_conn_unref(conn);

  return 0;
}

// 修改断开连接函数，确保用户断开不会自动重连
int ble_central_disconnect(uint8_t dev_index) {
  if (dev_index >= BLE_MAX_CONNECTIONS) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  // 取消所有挂起的重连和超时任务
  k_work_cancel_delayable(&devices[dev_index].connect_timeout_work);
  k_work_cancel_delayable(&devices[dev_index].retry_work);

  if (!devices[dev_index].connected && !devices[dev_index].connecting) {
    // 设备已断开或不在连接过程中
    // 清理UUID数据
    clean_device_uuid_data(&devices[dev_index]);
    devices[dev_index].in_use = false;
    k_mutex_unlock(&dev_mutex);
    return 0;
  }

  struct bt_conn *conn = devices[dev_index].conn;
  if (!conn && devices[dev_index].connecting) {
    // 设备正在连接中但尚未建立连接
    // 清理UUID数据
    clean_device_uuid_data(&devices[dev_index]);
    devices[dev_index].connecting = false;
    devices[dev_index].in_use = false;
    k_mutex_unlock(&dev_mutex);
    return 0;
  }

  // 临时禁用自动重连，因为这是用户主动断开
  bool auto_reconnect = devices[dev_index].auto_reconnect;
  devices[dev_index].auto_reconnect = false;

  // 清理UUID数据
  clean_device_uuid_data(&devices[dev_index]);

  k_mutex_unlock(&dev_mutex);

  // 断开连接
  int err = bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);

  // 恢复自动重连设置（只在断开失败时）
  if (err) {
    LOG_ERR("Disconnection failed (err %d)", err);

    k_mutex_lock(&dev_mutex, K_FOREVER);
    devices[dev_index].auto_reconnect = auto_reconnect;
    k_mutex_unlock(&dev_mutex);

    return err;
  }

  return 0;
}

// 新函数：设置自动重连
int ble_central_set_auto_reconnect(uint8_t dev_index, bool enable) {
  if (dev_index >= BLE_MAX_CONNECTIONS) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  devices[dev_index].auto_reconnect = enable;

  // 如果禁用自动重连，取消所有挂起的重连任务
  if (!enable) {
    k_work_cancel_delayable(&devices[dev_index].retry_work);
  }

  k_mutex_unlock(&dev_mutex);

  return 0;
}

// 新函数：获取连接状态
int ble_central_get_connection_state(uint8_t dev_index, bool *connecting,
                                     bool *connected) {
  if (dev_index >= BLE_MAX_CONNECTIONS) {
    return -EINVAL;
  }

  if (!connecting || !connected) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  *connecting = devices[dev_index].connecting;
  *connected = devices[dev_index].connected;

  k_mutex_unlock(&dev_mutex);

  return 0;
}

// 新函数：重置连接状态
int ble_central_reset_connection_state(uint8_t dev_index) {
  if (dev_index >= BLE_MAX_CONNECTIONS) {
    return -EINVAL;
  }

  k_mutex_lock(&dev_mutex, K_FOREVER);

  if (!devices[dev_index].in_use) {
    k_mutex_unlock(&dev_mutex);
    return -ENOENT;
  }

  // 只有在连接错误状态时才重置
  if (devices[dev_index].connecting && !devices[dev_index].connected) {
    LOG_INF("Resetting connection state for device %d", dev_index);

    // 取消所有挂起的任务
    k_work_cancel_delayable(&devices[dev_index].connect_timeout_work);
    k_work_cancel_delayable(&devices[dev_index].retry_work);

    devices[dev_index].connecting = false;
    devices[dev_index].reconnect_attempts = 0;

    // 清理UUID数据
    clean_device_uuid_data(&devices[dev_index]);

    // 如果有连接句柄，释放它
    if (devices[dev_index].conn) {
      bt_conn_disconnect(devices[dev_index].conn,
                         BT_HCI_ERR_REMOTE_USER_TERM_CONN);
      bt_conn_unref(devices[dev_index].conn);
      devices[dev_index].conn = NULL;
    }
  }

  k_mutex_unlock(&dev_mutex);

  return 0;
}
